/***************
 程序需要的头文件
****************/
#include"encoder.h"
#include"kinematics.hpp"
#include"motor.h"
#include"pid.hpp"
#include"WonderK210.h"
// #include<ServoTimer2.h>
// #include<Servo.h>

//------------------------------------舵机测试
// ServoTimer2 claw;
// Servo claw;

//------------------------------------k210模块声明部分
WonderK210* wk;     //接收控制器
Find_Box_st* result;//消息存储变量

//------------------------------------AGV底盘四轮声明，相关参数配置部分
MPID PIDA(encA,KP,KI,KD,true);
MPID PIDB(encB,KP,KI,KD,false);
MPID PIDC(encC,KP,KI,KD,true);
MPID PIDD(encD,KP,KI,KD,false);

void setup()
{
  delay(2000);          //程序开始部分
  Serial.begin(115200); //设置串口波特率
  motor_init();   //agv底盘小车引脚初始化
  k210_serial_init();   //k210串口初始化
  servo_init();         //舵机引脚初始化
  change_motor_action_time = millis();
  lastctrl=millis();
}

void loop(){
  search_for_ballon();
  search_for_field();
  cmdVelCb();
  delay(10);
}

void set_vel(float speed_vx, float speed_vy, float speed_vw){
  InverseKinematic(speed_vx, speed_vy, speed_vw, pwma, pwmb, pwmc, pwmd);
}

void servo_init(){
  // claw.attach(CLAW_PIN);
  // claw.write(map(CLAW_INIT, 0, 180, 750, 2250));
  // claw.write(CLAW_INIT);
  // delay(20);
  pinMode(CLAW_PIN,OUTPUT);
  servopulse(CLAW_INIT);
}

void motor_init(){
  IO_init();
  PIDA.init();
  PIDB.init();
  PIDC.init();
  PIDD.init();
  set_vel(0.0, 0.0, 0.0);
}

void k210_serial_init(){
  wk = malloc(sizeof(WonderK210));//分配内存
  //开启串口接收
  //这里不需要再调用初始化串口函数 Serial.begin(115200); 在 serial_begin() 函数里已经初始化了  
  wk->serial_begin();
  result = malloc(sizeof(Find_Box_st));//分配内存
}

//================================================================================================================================================
//小车寻找待抓物体==================================================================================================================================
//================================================================================================================================================
void search_for_ballon(){
  while(search_for_ballon_flag){
    wk->update_data();//更新消息
    bool rt = wk->recive_box(result , K210_FIND_RED_FOLLOW);//获取消息
    if(rt) //若获取到新的消息
    {
      rt = false;
      find_once_ballon_flag = true;
    }
    if(find_once_ballon_flag){
      find_something_ballon();
      // Serial.println("ballon_true");
      // Serial.println("find");
    }
    else{
      // Serial.println("left");
      // Serial.println("ballon_true");
      find_once_ballon_flag = false;
      set_target_vx = CAR_MOVE_STOP_SPEED;
      set_target_vy = CAR_MOVE_STOP_SPEED;
      set_target_vw = CAR_MOVE_VW_LEFT_TURN_SPEED_SLOWLY;
    }
    set_vel(set_target_vx, set_target_vy, set_target_vw);
    cmdVelCb();
    delay(10);
  }
}

void find_something_ballon(){
    int data_x = result->x + (result->w)/2;
    int data_y = result->y + (result->h)/2;

    //小车进入Vy方向路径判断
    if( (data_x >=0) && (data_x<128)){
      car_move_vy_direction = false;
      set_target_vy = CAR_MOVE_VY_LEFT_TRANSLATION_SPEED_SLOWLY;
    }
    else if( (data_x>=128) && (data_x<=192) ){
      car_move_vy_direction = true;
      set_target_vy = CAR_MOVE_STOP_SPEED;
    }
    else if( (data_x>192) && (data_x<=320) ){
      car_move_vy_direction = false;
      set_target_vy = CAR_MOVE_VY_RIGHT_TRANSLATION_SPEED_SLOWLY;
    }

    //小车进入Vx方向路径判断
    if(car_move_vy_direction)
    {
      if( (data_y>=0) && (data_y<210) ){
        set_target_vx = CAR_MOVE_VX_FORWARD_SPEED_SLOWLY;
      }
      else if( (data_y>=210) && (data_y<=240) ){
        set_target_vx = CAR_MOVE_STOP_SPEED;
        pick_up_object(1250);
        search_for_ballon_flag = false;
        search_for_field_flag = true;
        find_once_ballon_flag = false;
      }
    }
}

//================================================================================================================================================
//小车寻找红色场地=================================================================================================================================
//================================================================================================================================================
void search_for_field(){
  while(search_for_field_flag){
    wk->update_data();//更新消息
    bool rts = wk->recive_box(result , K210_COLOR_SORTING);//获取消息
    if(rts){
      rts = false;
      find_once_field_flag = true;
      // find_something_coordinate();
    }
    if(find_once_field_flag){
      // Serial.println("field_true");
      find_something_coordinate();
    }
    else{
      // Serial.println("field_false");
      find_once_field_flag = false;
      set_target_vx = CAR_MOVE_STOP_SPEED;
      set_target_vy = CAR_MOVE_STOP_SPEED;
      set_target_vw = CAR_MOVE_VW_LEFT_TURN_SPEED_SLOWLY;
    }
    set_vel(set_target_vx, set_target_vy, set_target_vw);
    cmdVelCb();
    delay(10);
  }
}

void find_something_coordinate(){
    int data_x = result->x + (result->w)/2;
    int data_y = result->y + (result->h)/2;
    //小车进入Vy方向路径判断
    if( (data_x >=0) && (data_x<128)){
      car_move_vy_direction = false;
      set_target_vy = CAR_MOVE_VY_LEFT_TRANSLATION_SPEED_SLOWLY;
    }
    else if( (data_x>=128) && (data_x<=192) ){
      car_move_vy_direction = true;
      set_target_vy = CAR_MOVE_STOP_SPEED;
    }
    else if( (data_x>192) && (data_x<=320) ){
      car_move_vy_direction = false;
      set_target_vy = CAR_MOVE_VY_RIGHT_TRANSLATION_SPEED_SLOWLY;
    }
    //小车进入Vx方向路径判断
    if(car_move_vy_direction)
    {
      if( (data_y>=0) && (data_y<140) ){
        set_target_vx = CAR_MOVE_VX_FORWARD_SPEED_SLOWLY;
      }
      else if( (data_y>=140) && (data_y<=240) ){
        set_target_vx = CAR_MOVE_STOP_SPEED;
        put_down_object(1250);
        search_for_field_flag = false;
        search_for_ballon_flag = true;
        find_once_field_flag = false;
      }
    }
    // wk->clear_rec();
}

void cmdVelCb(){
  PIDA.tic();
  MotorA(PIDA.getPWM(pwma));
  wA=PIDA.getWheelRotatialSpeed();
  PIDA.toc();

  PIDB.tic();
  MotorB(PIDB.getPWM(pwmb));
  wB=PIDB.getWheelRotatialSpeed();
  PIDB.toc();

  PIDC.tic();
  MotorC(PIDC.getPWM(pwmc));
  wC=PIDC.getWheelRotatialSpeed();
  PIDC.toc();

  PIDD.tic();
  MotorD(PIDD.getPWM(pwmd));
  wD=PIDD.getWheelRotatialSpeed();
  PIDD.toc();
  lastctrl=millis();
}

void car_move_test(){
  if( (millis() - change_motor_action_time) > 3000 ){
    car_move_status += 1;
    change_motor_action_time = millis();
    switch(car_move_status){
      case 1: {set_vel(-0.1,  0.0,  0.0);}break;        //forward
      case 2: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 3: {set_vel( 0.1,  0.0,  0.0);} break;       //back
      case 4: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 5: {set_vel( 0.0,  0.0,  0.5);} break;       //left
      case 6: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 7: {set_vel( 0.0,  0.0, -0.5);} break;       //right
      case 8: {set_vel( 0.0,  0.0,  0.0);} break;       //stop
      case 9: {set_vel( 0.0,  0.1,  0.0);} break;       //left_translation
      case 10: {set_vel( 0.0,  0.0,  0.0);} break;      //stop
      case 11: {set_vel( 0.0, -0.1,  0.0);} break;      //right_translation
      case 12: {set_vel( 0.0,  0.0,  0.0);} break;      //stop
      default: break;
    }
    if(car_move_status >= 12){
      car_move_status = 0;
    }
  }
}


void pick_up_object(unsigned long delay_time){
  //让小车停下来
  unsigned long time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < 2000 ){
    cmdVelCb();
    delay(10);
  }

  claw_close();delay(2000);
  // claw_open();delay(2000);
  wk->clear_rec();
}

void put_down_object(unsigned long delay_time){
  //让小车停下来
  unsigned long time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < 2000 ){
    cmdVelCb();
    delay(10);
  }
  claw_open();delay(2000);
  wk->clear_rec();

  far_away_put_area(3000, 3200);
}

void far_away_put_area(unsigned long back_delay_time, unsigned long left_delay_time){
  //让小车停下来
  unsigned long time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < 2000 ){
    cmdVelCb();
    delay(10);
  }
  wk->clear_rec();

  time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_VX_BACK_SPEED_SLOWLY;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < back_delay_time ){
    cmdVelCb();
    delay(10);
  }
  wk->clear_rec();

  time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < 2000 ){
    cmdVelCb();
    delay(10);
  }
  wk->clear_rec();


  time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_VW_LEFT_TURN_SPEED_SLOWLY;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < left_delay_time ){
    cmdVelCb();
    delay(10);
  }
  wk->clear_rec();

  time_now = millis();
  set_target_vy = CAR_MOVE_STOP_SPEED;
  set_target_vw = CAR_MOVE_STOP_SPEED;
  set_target_vx = CAR_MOVE_STOP_SPEED;
  set_vel(set_target_vx, set_target_vy, set_target_vw);
  while( (millis() - time_now) < 2000 ){
    cmdVelCb();
    delay(10);
  }
  wk->clear_rec();
}

void claw_open(){
  servopulse(CLAW_OPEN);
}

void claw_close(){
  servopulse(CLAW_CLOSE);
}

void servopulse(int angle)//定义一个脉冲函数
{
  int pulsewidth=(angle*11)+500;  //将角度转化为500-2480的脉宽值
  digitalWrite(CLAW_PIN,HIGH);    //将舵机接口电平至高
  delayMicroseconds(pulsewidth);  //延时脉宽值的微秒数
  digitalWrite(CLAW_PIN,LOW);     //将舵机接口电平至低
  delayMicroseconds(20000-pulsewidth);
}

